#include <Arduino.h>
#include "definicion.h"
#include "libreria/percepcion.h"
#include "libreria/comunicacionSerial.h"

percepcionLR2 sistemaPercepcion;
comunicacionSerial comSerial;
char sbuf[4];
int id, anbgulo = 90;
unsigned short int distanciaUS = 0, *distanciasUS;
uint8_t distanciaIR, *distanciasIR;
uint8_t wbuf[12];

void serializeShort(uint8_t *buf, unsigned short int value) {
   union{
	   uint8_t c[2];
       unsigned short int val;
   } unsigned_short_int;
   if (buf == NULL) { return; }
   unsigned_short_int.val = value;
   buf[0] = unsigned_short_int.c[1];
   buf[1] = unsigned_short_int.c[0];
}

void setup() {
	sistemaPercepcion.inicializarSistemaPerecepcion();
	comSerial.iniciarComunicacion(BAUDIOS_SERIAL);
}

void loop() {
	if(comSerial.verificarBufer()){
		delay(DELAY_SERIAL);
		comSerial.leerDatos(BYTE_FIN_COMADO, sbuf, SIZE_MAX_COMADO);
		comSerial.limpiarBufer();
		if(sbuf[0] == BYTE_INICIO_COMADO){
			switch (sbuf[1]) {
				case OBTENER_MEDIDA_US:
					id = (int)sbuf[2];
					distanciaUS = sistemaPercepcion.obtenerMedidaUS(id);
					serializeShort(wbuf,distanciaUS);
					comSerial.escribirDatos(2,wbuf);
					break;
				case OBTENER_MEDIDA_IR:
					id = (int)sbuf[2];
					distanciaIR = sistemaPercepcion.obtenerMedidaIR(id);
					wbuf[0] = (uint8_t)distanciaIR;
					comSerial.escribirDatos(1,wbuf);
					break;
				case OBTENER_MEDIDA_UST:
					anbgulo = (int)sbuf[2];
					distanciaUS = sistemaPercepcion.obtenerMedidaUSTrasero(anbgulo);
					serializeShort(wbuf,distanciaUS);
					comSerial.escribirDatos(2,wbuf);
					break;
				case OBTENER_BARRIDO_FRONTAL_US:
					distanciasUS = sistemaPercepcion.obtenerBarridoFrontalUS();
					for (int i = 0; i < 6; ++i) {
						serializeShort(&wbuf[2*i],distanciasUS[i]);
					}
					comSerial.escribirDatos(12,wbuf);
					break;
				case OBTENER_BARRIDO_FRONTAL_IR:
					distanciasIR = sistemaPercepcion.obtenerBarridoFrontalIR();
					for (int i = 0; i < 6; ++i) {
						wbuf[i] = (uint8_t)distanciasIR[i];
					}
					comSerial.escribirDatos(6,wbuf);
					break;
				case OBTENER_BARRIDO_TRASERO:
					distanciasUS = sistemaPercepcion.obtenerBarridoTraseroUS();
					for (int i = 0; i < 6; ++i) {
						serializeShort(&wbuf[2*i],distanciasUS[i]);
					}
					comSerial.escribirDatos(10,wbuf);
					break;
				default:
					comSerial.limpiarBufer();
					break;
			}
		}else{
			comSerial.limpiarBufer();
		}
	}
}

int main(void){
	init();
	setup();
	for (;;)
		loop();
	return 0;
}
